#ifndef CNC_BASE_COOPERATIVE_SCHEDULER_H
#define CNC_BASE_COOPERATIVE_SCHEDULER_H

//#include <iostream>
#include "taskstate.h"
#include <vector>
#include <map>
#include <cassert>

namespace cnc {

	namespace base {

		template <typename OperationModeID, class BackgroundTask, class OperationMode>
		class CooperativeScheduler
		{
			typedef std::vector<BackgroundTask *> TaskVector_;
			typedef std::map<OperationModeID, OperationMode *> ModeMap_;
			TaskVector_ backgroundTask_;
			ModeMap_ operationMode_;
			OperationModeID thisMode_;
			OperationModeID nextMode_;
			bool modeSelected_;
			bool execute_;
		public:
			CooperativeScheduler() : modeSelected_(false) {}
			void addTask(BackgroundTask *backgroundTask)
			{
				backgroundTask_.push_back(backgroundTask);
			}
			void addMode(OperationModeID modeID, OperationMode *operationMode)
			{
				operationMode_[modeID] = operationMode;
			}
			void selectMode(OperationModeID modeID)
			{
				modeSelected_ = true;
				nextMode_ = modeID;
			}
			OperationModeID getMode()
			{
				return thisMode_;
			}
			bool start()
			{
				try {
					for (unsigned int i = 0; i < backgroundTask_.size(); i++)
						backgroundTask_[i]->start();
					return true;
				} catch (...) {
					return false;
				}
			}
			bool stop()
			{
				try {
					for (unsigned int i = 0; i < backgroundTask_.size(); i++)
						backgroundTask_[i]->stop();
					return true;
				} catch (...) {
					return false;
				}
			}
			bool execute()
			{
				if (modeSelected_ == false)
					return false;
				execute_ = true;
				thisMode_ = nextMode_;
				operationMode_[thisMode_]->setState(TASKSTATE_STARTING);
				try {
					while (execute_) {
						switch (operationMode_[thisMode_]->getState()) {
						case TASKSTATE_STARTING:
							operationMode_[thisMode_]->start();
							operationMode_[thisMode_]->setState(TASKSTATE_ACTIVE);
							break;
						case TASKSTATE_ACTIVE:
							if (operationMode_[thisMode_]->execute() == false)
								operationMode_[thisMode_]->setState(TASKSTATE_STOPPING);
							break;
						case TASKSTATE_STOPPING:
							operationMode_[thisMode_]->stop();
							operationMode_[thisMode_]->setState(TASKSTATE_IDLE);
							break;
						case TASKSTATE_IDLE:
							if (nextMode_ == thisMode_)
								execute_ = false;
							else {
								thisMode_ = nextMode_;
								operationMode_[thisMode_]->setState(TASKSTATE_STARTING);
							}
							break;
						}
						for (unsigned int i = 0; i < backgroundTask_.size(); i++)
							backgroundTask_[i]->execute();
					}
					return true;
				} catch (...) {
					return false;
				}
			}

		};

	} // namespace base

} // namespace cnc

#endif

